#include <ProcessComposition.h>
#include <shared_data.h>
#include <csp_shared_types.h>
#include <csp_protocol_msg.h>
#include <csp_debug.h>
#include <Scheduler.h>
#include <ProtocolHandler.h>
#include <LocalStoreManager.h>
#include <errno.h>
#include <ppu_intrinsics.h>
#include <WaitQueueEvent.h>
#include <Alting.h>
#include <StopWatch.h>

#define NO_PROCESS_SWAP -22

ProtocolHandler::ProtocolHandler(spe_context_ptr_t *ctx_, spe_args *spe_setup)
{
	ctx = ctx_;
	pcb_pointers = (ea_addr_t*) spe_setup->process_data_EA;
	spe_id = spe_setup->spe_id;
	spe_offload_area = (uint64_t *) spe_setup->offload_area_ea;
	pendingEvent = NULL;

	lsManager = new LocalStoreManager(spe_id);

	channelManager = ChannelManager::Instance();

	scheduler = Scheduler::Instance();

	p_swap_out = NO_PROCESS_SWAP;

	last_channel_id = -1;
	last_channel = NULL;


}

void 
ProtocolHandler::mbox_write(uint32_t sig){
	uint32_t msg = sig;
	int status = spe_in_mbox_write(*ctx, &msg, 1, SPE_MBOX_ALL_BLOCKING);
	if(1 != status){
		perror("Command to SPE not delivered.");
		exit(1);
	}
}

	void
ProtocolHandler::read_mbox_blocking(uint32_t* msg)
{
	spe_out_intr_mbox_read(*ctx, msg, 1, SPE_MBOX_ALL_BLOCKING);
}


int
ProtocolHandler::ChannelCommunication(uint32_t slot){
	//channel_link_t* link;
	Channel* channel;
	csp_channel_t *chan_pack = (csp_channel_t*) spe_offload_area;
	ProcessComposition *correspondingProcess;
	ProcessComposition *process = active_procs[slot];

	//__lwsync();

	//if( last_channel_id == chan_pack->id ){
	//	printf("JA: last_channel_id=%li, chan_pack->id=%lx\n", last_channel_id, chan_pack->id);
	//}else{
	//	printf("NEJ: last_channel_id=%li, chan_pack->id=%lx\n", last_channel_id, chan_pack->id);
	//}

	channel = channelManager->GetChannel(chan_pack->type, chan_pack->id);
	//last_channel_id = chan_pack->id;

	channel->Lock();


	ASSERT( channel );
	ASSERT( CHANNEL_WRITE == chan_pack->op || CHANNEL_READ == chan_pack->op);

	ASSERT( 0 == process->GetPCB()->dma_completion_flag[0] );


	if( channel->IsPoisoned() ){

		channel->Unlock();

		//printf("Process (pid=%i) is denied op on poisoned channel\n", 
		//		process->GetPID());

		mbox_write( COMM_POISON );

		return WAIT_FOR_SPE_MSG;
	}


	char *c_op;
	if( CHANNEL_WRITE == chan_pack->op ){
		correspondingProcess = channel->Write(process, chan_pack->addr, chan_pack->nbytes);
		c_op = "write";
	}else if( CHANNEL_READ == chan_pack->op ){
		correspondingProcess = channel->Read(process, chan_pack->addr, chan_pack->nbytes);
		c_op = "read";
	}else{
		_PRINT_EXIT("Unknown channel-operation.\n")
	}

	channel->Unlock(); /* Remember! */



	ASSERT( chan_pack->id );

	if( correspondingProcess ){ /* Corresponding op has happened - act! */
		DEBUG_PRINTF("Process: %s (pid=%i) link established to %i on channel=%lld.\n", 
				c_op, process->GetPID(), correspondingProcess->GetPID(), chan_pack->id);
		//correspondingProcess = link->process;

		correspondingProcess->WaitForDMACompletion(1, spe_id); /* Make sure process is in EA. */

		ASSERT( STATE_CHAN_WAIT == correspondingProcess->GetState() );

		ASSERT( 0 == process->GetPCB()->dma_completion_flag[0] );
		ASSERT( 0 == process->GetPCB()->dma_completion_flag[1] );

		/* Channel-data at corr. process. */
		correspondingProcess->GetPCB()->channel_data = 
			LS_ADDR_IN_EASTORE(correspondingProcess->GetPCB(), 
				correspondingProcess->channel_link.ls_addr);

		/* Corr. sponding process is first valid when channel-op is finished. */
		correspondingProcess->ClearDMACompletionFlag();

		/* Corr. sponding process should be resumed when scheduled on SPE. */
		correspondingProcess->GetPCB()->state = STATE_YIELD;


		/* Notify both processes that operation was a success. */
		correspondingProcess->GetPCB()->op_ret_val = 
			correspondingProcess->channel_link.return_value;
		process->GetPCB()->op_ret_val =  CHANNEL_SUCCESS;

		/* This process reads correspondings pcb to read return value. */
		correspondingProcess->GetPCB()->misc_val = CHANNEL_SUCCESS;


		/*Copy corr. process' pcb - so SPE can start transfer. */
		//memcpy((void*)spe_offload_area, correspondingProcess->GetPCB(),
		//		sizeof(pcb_t));

		/* Notify SPE. */
		//mbox_write( COMM_CHANN_YES | CHANNEL_SUCCESS );
		mbox_write( COMM_CHANN_YES |  correspondingProcess->GetPCBIdx());

		/* Make process ready for scheduling. */
/*
		correspondingProcess->SetLast(3, spe_id);
*/
		scheduler->put(*correspondingProcess, correspondingProcess->GetState());

	}else{ /* No corresponding op - yield. */
		DEBUG_PRINTF("Process: %s (pid=%i) no link on channel=%llX: yield.\n", 
				c_op, process->GetPID(), chan_pack->id);
		ASSERT( 0 == process->GetPCB()->dma_completion_flag[0] );
		ASSERT( 0 == process->GetPCB()->dma_completion_flag[1] );
/*
		process->SetLast(5, spe_id);
*/
		mbox_write( COMM_CHANN_NO_YIELD );
	}


	return WAIT_FOR_SPE_MSG;
}

int
ProtocolHandler::TransferProcs(){
	ProcessComposition *p;
	int count = 0;

	//printf("spe-id: %i\n", spe_id);

try_again:
	while( NULL != (p = lsManager->Get()) ){

		p->WaitForDMACompletion(2, spe_id);
		p->ClearDMACompletionFlag();

		DEBUG_PRINTF("PPC #%i: found complete process (pid=%i) in EA!\n", spe_id, p->GetPID());

		pcb_pointers[p->GetMemorySlot()] = (ea_addr_t) p->GetPCB();
		active_procs[p->GetMemorySlot()] = p;

		p->SetLastSpeID(spe_id);

		mbox_write( COMM_TAKE_PROC | p->GetMemorySlot() );
		count++;

		p_swap_out = NO_PROCESS_SWAP;
	}

	if( !count && !lsManager->LiveProcesses() ){
		DEBUG_PRINTF("#%i: COMM_KILL\n", spe_id);
		mbox_write(COMM_KILL);	
		return COMM_KILL;

	} else if(!count && !lsManager->GetCount() && NO_PROCESS_SWAP == p_swap_out ){
		//sw_transfer_procs.start();
		usleep(1000); // TODO!	
		//sw_transfer_procs.stop();
		goto try_again;
	}

	/* This means that SPE needs to write a process, but there was
	 * no process to swap in. Therefore we let SPE write process and
	 * start all over again until a process is available. */
	/*
	   if( p_swap_out != NO_PROCESS_SWAP ){
	   mbox_write( COMM_WRITE_PROCESS | p_swap_out );	
	   p_swap_out = NO_PROCESS_SWAP;

	   return FETCH_FROM_SCHEDULER;
	   }
	   */

	//printf("Localstore size: %i\n", lsManager->GetCount());

	mbox_write(COMM_EXECUTE);	
	return WAIT_FOR_SPE_MSG;
}

int 
ProtocolHandler::TakeProcess(uint32_t slot, uint32_t state){
	//__lwsync();
	ProcessComposition *p = active_procs[slot];

	/*
	   if( STATE_EXIT != state ){
	   p_swap_out = slot;
	   }
	   */


	/* Active event that schedules process after wait. */
	if( STATE_WAIT == state ){
		pendingEvent->SetActive();	
		pendingEvent = NULL;
	}

	lsManager->Put(*p, state);
	return FETCH_FROM_SCHEDULER;
}

	int
ProtocolHandler::StartProcesses(uint32_t slot)
{
	uint64_t mnemonic_hash;

	/* Make sure that spe-write to EA has completed. */
	//__lwsync();
	int proc_count = spe_offload_area[0];

	extern CSPConfig *configuration;

	WaitQueueEvent *event = new WaitQueueEvent(proc_count, active_procs[slot]);

	for(int i = 0; i < proc_count; i++){
		mnemonic_hash = spe_offload_area[i+1];

		if( !configuration->ProgramExists(mnemonic_hash) ){
			printf("CSP-process (%s, pid=%i) tried to start process with unknown mnemonic.\n", 
					active_procs[slot]->GetMnemonic(), active_procs[slot]->GetPID() );
			exit(-1);
		}

		ProcessComposition *p = new ProcessComposition(mnemonic_hash, event, NULL);
		p->Schedule();
	}

	pendingEvent = event;

	return WAIT_FOR_SPE_MSG;
}


int ProtocolHandler::Alting(uint32_t slot, int n_guards)
{
	int i;
	//channel_link_t *link;
	ProcessComposition *process, *correspondingProcess;
	csp_channel_t *channels = (csp_channel_t*) 
		_malloc_align(sizeof(csp_channel_t)*n_guards, 7);

	/* Move from offload area. */
	memcpy(channels, spe_offload_area, sizeof(csp_channel_t)*n_guards);

	process = active_procs[slot];

	Alting::Alting *alt = process->GetAlting(channels, n_guards);


	//link = alt->PriSelect();
	correspondingProcess = alt->PriSelect();


	if( correspondingProcess ){ /* Corresponding op has happened - act! */
		printf("Got (pid=%i) link from pid=%i\n", 
				active_procs[slot]->GetPID(), correspondingProcess->GetPID() );	
		/*	
			printf("Process: %s (pid=%i) link established to %i on channel=%lld.\n", 
			"alting_read", process->GetPID(), link->process->GetPID(), chan_pack->id);
			*/
		//correspondingProcess = link->process;

		correspondingProcess->WaitForDMACompletion(1, spe_id); /* Make sure process is in EA. */

		ASSERT( STATE_CHAN_WAIT == correspondingProcess->GetState() );

		ASSERT( 0 == process->GetPCB()->dma_completion_flag[0] );
		ASSERT( 0 == process->GetPCB()->dma_completion_flag[1] );

		/* Channel-data at corr. process. */
		correspondingProcess->GetPCB()->channel_data = 
			LS_ADDR_IN_EASTORE(correspondingProcess->GetPCB(), 
				correspondingProcess->channel_link.ls_addr);
		printf("Remote process (pid=%i) ls=%x, EA=%x\n", 
				process->GetPID(), correspondingProcess->channel_link.ls_addr, 
				correspondingProcess->GetPCB()->channel_data);

		/* Corr. sponding process is first valid when channel-op is finished. */
		correspondingProcess->ClearDMACompletionFlag();

		/* Corr. sponding process should be resumed when scheduled on SPE. */
		correspondingProcess->GetPCB()->state = STATE_YIELD;


		/* Notify corr. processes that operation was a success. */
		correspondingProcess->GetPCB()->op_ret_val = CHANNEL_SUCCESS;

		/* This process reads correspondings pcb to read return value. */
		correspondingProcess->GetPCB()->misc_val = process->GetPCB()->op_ret_val;

		/*Copy corr. process' pcb - so SPE can start transfer. */
		//memcpy((void*)spe_offload_area, correspondingProcess->GetPCB(),
		//		sizeof(pcb_t));

		/* Notify SPE. */
		//mbox_write( COMM_CHANN_YES |  process->GetPCB()->op_ret_val);
		mbox_write( COMM_CHANN_YES |  correspondingProcess->GetPCBIdx());

		/* Make process ready for scheduling. */
/*
		correspondingProcess->SetLast(3, spe_id);
*/
		scheduler->put(*correspondingProcess, correspondingProcess->GetState());

	}else{ /* No corresponding op - yield. */
		printf("Did (pid=%i) not get link\n", 
				active_procs[slot]->GetPID() );	

		DEBUG_PRINTF("Process: %s (pid=%i) no link on channel=%llX: yield.\n", 
				c_op, process->GetPID(), chan_pack->id);
		ASSERT( 0 == process->GetPCB()->dma_completion_flag[0] );
		ASSERT( 0 == process->GetPCB()->dma_completion_flag[1] );
/*
		process->SetLast(5, spe_id);
*/
		mbox_write( COMM_CHANN_NO_YIELD );
	}


	return WAIT_FOR_SPE_MSG;
}





int ProtocolHandler::Poison()
{

	long type, id;
	Channel* channel;
	csp_channel_t *chan_pack = (csp_channel_t*) spe_offload_area;

	type = chan_pack->type; id = chan_pack->id;

	/* We have read the values in offload_ea, SPE can execute again. */
	//mbox_write(COMM_EXECUTE);	

	channel = channelManager->GetChannel(type, id);

	channel->Lock();

	channel->Poison();

	channel->Unlock();

	return WAIT_FOR_SPE_MSG;
}


void ProtocolHandler::Talk()
{
	uint32_t message = FETCH_FROM_SCHEDULER;
	uint32_t option = 0;
	uint32_t state  = 0;
	char buf[256];
	long microseconds;
	long timePerLoop_us;

	int slot=0, n_channels=0;

	//sw_transfer_procs.clear(); sw_talk.clear();
	//sw_talk.start();

	//StopWatch sw_alt, sw_channel, sw_put, sw_take;
	//int c_alt=0, c_channel=0, c_put=0, c_take=0;
	
	extern StopWatch sw_spu;

	/*****       Start communicating with SPE      ******/
	int thread_alive = 1;
	while(thread_alive){
		option  = MSG_OPT(message); /* arg #3 */
		state   = MSG_STATE(message); /* arg #2 */
		message = MSG_CMD(message); /* arg #1*/

		switch( message ) {

			case COMM_ALT:
				slot = state >> 12; /* In this case state is number, not a pattern. */
				printf("state=%x\n", state);
				n_channels = option;
				printf("#%d: COMM_ALT slot=%i, n_guards=%i (pid=%i)\n", 
						spe_id, slot, n_channels,  active_procs[slot]->GetPID());

				//sw_alt.start();
				//c_alt++;
				message = Alting(slot, n_channels);
				//sw_alt.stop();


				break;
			case FETCH_FROM_SCHEDULER:
				DEBUG_PRINTF("#%d: FETCH_FROM_SCHEDULER\n", spe_id);

				//sw_take.start();
				//c_take++;
				message = TransferProcs();
				//sw_take.stop();

				break;
			case COMM_POISON:
				slot = option;
				DEBUG_PRINTF("#%d: COMM_POISON (slot=%i, pid=%i)\n", 
						spe_id, slot, active_procs[slot]->GetPID() );

				message = Poison();

				break;
			case COMM_CHANN_TALK:

				//sw_channel.start();
				//c_channel++;
				message = ChannelCommunication(option);
				//sw_channel.stop();

				break;
			case WAIT_FOR_SPE_MSG:
				/* Wait for interrupt */	
				DEBUG_PRINTF("#%d: WAIT_FOR_SPE_MSG\n", spe_id);
				message = 0;
				read_mbox_blocking(&message);
				DEBUG_PRINTF("Got message=%x\n", message);

				break;
			case COMM_INIT_PROCS:
				message = StartProcesses(option);
				break;
			case COMM_PUT_PROC:
				DEBUG_PRINTF("#%d: COMM_PUT_PROC #%i\n", spe_id, option);

				//sw_put.start();
				//c_put++;
				message = TakeProcess(option, state);
				//sw_put.stop();

				break;
			case COMM_KILL:
				DEBUG_PRINTF("%d: COMM_KILL: stopping thread loop.\n", spe_id);
				thread_alive = 0;
				break;
			case COMM_DESTROY:
				DEBUG_PRINTF("%d: COMM_DESTROY: Network destroyed.", spe_id);
				//exit(0);
				thread_alive = 0;
				break;
			case COMM_SW_START:
				printf("hej3\n");
				sw_spu.clear();
				sw_spu.start();
				message = WAIT_FOR_SPE_MSG;
				break;

			case COMM_SW_STOP:
				sw_spu.stop();
				sw_spu.timeString(buf);
#define N_LOOPS 10000
				microseconds = ((sw_spu.readSeconds()*1000)+sw_spu.readmSeconds())*1000;
				timePerLoop_us = (long) microseconds / 10000.0;
				printf("   %li microseconds / iteration\n", timePerLoop_us);
				timePerLoop_us = (microseconds / ((long) (4*N_LOOPS)));
				printf("   %li microseconds / communication\n", timePerLoop_us);
				timePerLoop_us = (microseconds / ((long) (8*N_LOOPS)));
				printf("   %li microseconds / context switch\n", timePerLoop_us);
				printf("%s\n", buf);
				message = WAIT_FOR_SPE_MSG;
				break;
			default  : 
				printf("Protocol error in %s. MSG=%X, STATE=%X, OPTION=%X\n",__FILE__, message, state, option);
				exit(-1);
				break;
		}	
	}

//	sw_talk.stop();
//
//	char sw_alt_buf[256];
//	char sw_channel_buf[256];
//	char sw_put_buf[256];
//	char sw_take_buf[256];
//	char sw_transfer_procs_buf[256];
//	char sw_talk_buf[256];
//
//
//
//	sw_alt.timeString(sw_alt_buf);
//	sw_channel.timeString(sw_channel_buf);
//	sw_put.timeString(sw_put_buf);
//	sw_take.timeString(sw_take_buf);
//	sw_transfer_procs.timeString(sw_transfer_procs_buf);
//	sw_talk.timeString(sw_talk_buf);
//
//
//	printf("STATUS (%i):\n"
//			"\talting(%i): %s\n"	
//			"\tchannel(%i): %s\n"	
//			"\tput(%i): %s\n"	
//			"\ttake(%i): %s\n"
//			"\ttalk: %s\n"
//			"\tDMA: %s\n", 
//			spe_id,
//			c_alt, sw_alt_buf, 
//			c_channel, sw_channel_buf, 
//			c_put, sw_put_buf, 
//			c_take, sw_take_buf,
//			sw_talk_buf,
//			sw_transfer_procs_buf);
}
